#ifndef ROBORTS_DECISION_BLOOD_BEHAVIOR_H
#define ROBORTS_DECISION_BLOOD_BEHAVIOR_H

#include "io/io.h"

#include "../blackboard/blackboard.h"
#include "../executor/chassis_executor.h"
#include "../behavior_tree/behavior_state.h"
#include "../proto/decision.pb.h"

#include "line_iterator.h"

namespace roborts_decision {
class BloodBehavior{
  public:
  BloodBehavior(ChassisExecutor* &chassis_executor,
		Blackboard* &blackboard,
	const std::string & proto_file_path) : chassis_executor_(chassis_executor),
                                                       blackboard_(blackboard) {
    if (!LoadParam(proto_file_path)) {
      ROS_ERROR("%s can't open file", __FUNCTION__);
    }}

  void Cancel() {
    chassis_executor_->Cancel();
  }

  BehaviorState Update() {
    return chassis_executor_->Update();
  }

bool LoadParam(const std::string &proto_file_path)
{
roborts_decision::DecisionConfig decision_config;
    if (!roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config)) {
      return false;
    }
  for(int i=0;i<6;i++)
  {
	buff_position_[i].header.frame_id="map";
buff_position_[i].pose.position.x=decision_config.buff_point(i).x();
buff_position_[i].pose.position.y=decision_config.buff_point(i).y();
buff_position_[i].pose.position.z=decision_config.buff_point(i).z();
      tf::Quaternion quaternion = tf::createQuaternionFromRPY(decision_config.buff_point(i).roll(),
                                                              decision_config.buff_point(i).pitch(),
                                                              decision_config.buff_point(i).yaw());
buff_position_[i].pose.orientation.x=quaternion.x();
buff_position_[i].pose.orientation.y=quaternion.y();
buff_position_[i].pose.orientation.z=quaternion.z();
buff_position_[i].pose.orientation.w=quaternion.w();
}
}

  void Run(int i)
{
auto executor_state = Update();
if (in_circle==0 && executor_state != BehaviorState::RUNNING && executor_state != BehaviorState::FAILURE) {
chassis_executor_->Execute(buff_position_[i], Behaviortype::BLOOD);
}
else if((in_circle||executor_state == BehaviorState::FAILURE))
{
if(in_circle==0)
    {
       begin_time = ros::Time::now();
     }
     in_circle=1;
     roborts_msgs::TwistAccel t;
      t.twist.angular.z = 1.57;
      chassis_executor_->Execute(t);
       if(ros::Time::now().sec-begin_time.sec>=4)in_circle=0;
}
}



~BloodBehavior(){Cancel();}

private:
  ChassisExecutor* const chassis_executor_;
  Blackboard* const blackboard_;
  geometry_msgs::PoseStamped buff_position_[8];
  int in_circle;
  ros::Time begin_time;
};
}
#endif